
/***************************************************************************************************************************
* LR_controller_cascade_PID.h
*
* Author: SFL
*
* Update Time: 2023.7.25
*
* Introduction:  Left Right Controller using PID (P for pos loop, pid for vel loop) in body frame
*         1. Similiar to the position controller in PX4 (1.8.2)
*         2. Ref to : https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/PositionControl.cpp
*         3. 直接将像素偏差映射到目标姿态角上，专为无GPS情况下跟踪红框所用
***************************************************************************************************************************/



#ifndef LR_CONTROLLER_CASCADE_PID_H
#define LR_CONTROLLER_CASCADE_PID_H

#include <Eigen/Eigen>
#include <math.h>
#include <math_utils.h>
#include <px4_command_utils.h>
#include <wrzf_pkg/DroneState.h>

using namespace std;

class LR_controller_cascade_PID
{
public:
    LR_controller_cascade_PID(void):
        lr_cascade_pid_nh("~")
    {
        lr_cascade_pid_nh.param<float>("LR_cascade_pid/Kp_y_b", Kp_y_b, 0.005);
        lr_cascade_pid_nh.param<float>("LR_cascade_pid/Kp_vy_b", Kp_vy_b, 0.4);
        lr_cascade_pid_nh.param<float>("LR_cascade_pid/Ki_vy_b", Ki_vy_b, 0.02);
        lr_cascade_pid_nh.param<float>("LR_cascade_pid/Kd_vy_b", Kd_vy_b, 0.15);

        lr_cascade_pid_nh.param<float>("LR_cascade_pid/ROLL_MAX", ROLL_MAX, 0.25);
        lr_cascade_pid_nh.param<float>("LR_cascade_pid/ACC_MAX", ACC_MAX, 3.0);
        lr_cascade_pid_nh.param<float>("LR_cascade_pid/VY_B_MAX", VY_B_MAX, 3.0);

        vel_setpoint        = 0.0;
        roll_sp             = 0.0;
        acc_sp              = 0.0;
        vel_P_output        = 0.0;
        acc_int            = 0.0;
        vel_D_output        = 0.0;
        error_vel_dot_last  = 0.0;
        error_vel_last      = 0.0;
        error_vel           = 0.0;
        vel_error_deriv     = 0.0;
        delta_time          = 0.02;
    }

    // PID parameter for the control law
    float Kp_y_b;
    float Kp_vy_b;
    float Ki_vy_b;
    float Kd_vy_b;

    // limit 
    float ROLL_MAX;
    float ACC_MAX;
    float VY_B_MAX;

    // 中间控制量与目标滚转角
    float vel_setpoint;
    float roll_sp;
    float acc_sp;
    float vel_P_output;
    float acc_int;
    float vel_D_output;
    float error_vel_dot_last;
    float error_vel_last;
    float error_vel;
    float vel_error_deriv;
    float delta_time;

    // Position control main function 
    // [Input: cam_error, DroneState, dt; Output: roll_sp;]
    float pos_controller(const int cam_error, const wrzf_pkg::DroneState& _DroneState, float dt);

    //Position control loop [Input: cam_error; Output: desired vel]
    void _positionController(const int cam_error, float& vel_setpoint);

    //Velocity control loop [Input: current vel, desired vel; Output: desired roll]
    void _velocityController(const wrzf_pkg::DroneState& _DroneState, float& acc_sp);

    void _acc_to_roll(float& acc_sp, float& roll_sp);

    void cal_vel_error_deriv(const float& error_now, float& vel_error_deriv);

private:
    ros::NodeHandle lr_cascade_pid_nh;

};



float LR_controller_cascade_PID::pos_controller(const int cam_error, const wrzf_pkg::DroneState& _DroneState, float dt)
{
    delta_time = dt;

    _positionController(cam_error, vel_setpoint);

    _velocityController(_DroneState, acc_sp);

    _acc_to_roll(acc_sp, roll_sp);

    return roll_sp;
}

void LR_controller_cascade_PID::_positionController(const int cam_error, float& vel_setpoint)
{
    vel_setpoint = Kp_y_b  * cam_error;
    vel_setpoint = constrain_function(vel_setpoint, VY_B_MAX);
}

void LR_controller_cascade_PID::_velocityController(const wrzf_pkg::DroneState& _DroneState, float& acc_sp)
{
    error_vel = vel_setpoint - _DroneState.velocity_body[1];
    vel_P_output = Kp_vy_b * error_vel;
    cal_vel_error_deriv(error_vel, vel_error_deriv);
    vel_D_output = Kd_vy_b  * vel_error_deriv;
    float acc_desired_yb = vel_P_output + vel_D_output + acc_int;
    bool stop_integral_yb = (acc_desired_yb >= ACC_MAX && error_vel >= 0.0f) ||
                            (acc_desired_yb <= -ACC_MAX && error_vel <= 0.0f);
    if (!stop_integral_yb)
    {
        acc_int += Ki_vy_b * error_vel * delta_time;

        // 积分限幅
        acc_int = min(fabs(acc_int), ACC_MAX) * sign_function(acc_int);
    }
    acc_sp = constrain_function(acc_desired_yb, ACC_MAX);
}

void LR_controller_cascade_PID::_acc_to_roll(float& acc_sp, float& roll_sp)
{
    roll_sp = atan2(-acc_sp, 9.8);
    roll_sp = constrain_function(roll_sp, ROLL_MAX);
}

void LR_controller_cascade_PID::cal_vel_error_deriv(const float& error_now, float& vel_error_deriv)
{
    float error_vel_dot_now;
    error_vel_dot_now = (error_now - error_vel_last)/delta_time;

    error_vel_last = error_now;
    float a,b;
    b = 2 * M_PI * 5 * delta_time;
    a = b / (1 + b);

    vel_error_deriv = a * error_vel_dot_now + (1 - a) * error_vel_dot_last ;

    error_vel_dot_last = vel_error_deriv;
}


#endif //LR_CONTROLLER_CASCADE_PID_H
